//=============================================================================
// Disclaimer - Exclusion of Liability
//
// This software is distributed in the hope that it will be useful,but WITHOUT 
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
// FITNESS FOR A PARTICULAR PURPOSE. It can be used an modified by anyone
// free of any license obligations or authoring rights.
//=============================================================================

using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;

namespace MotorStop {

    public enum MotorIdx { MOTOR_1 = 0, MOTOR_2, MOTOR_3, MOTOR_4 };
    public enum InpIdx { I1 = 0, I2, I3, I4, I5, I6, I7, I8 };
    public enum CntIdx { COUNT_1 = 0, COUNT_2, COUNT_3, COUNT_4 };

    public partial class Program {

        const int  N_MOTOR   = 4;
        const bool MOTOR_ON  = true;
        const bool MOTOR_OFF = false;

        const int  DUTY_MIN  = 0;
        const int  DUTY_MAX  = 512;

        static uint ftHandle = 0;

        public static void Main(string[] args) {

            Console.WriteLine("Example MotorStop.exe ...");

            //  check input paramter
            if (!ChckCOMPar(args))
                return;

            //  get library version
            Console.WriteLine("\nftMscLib {0}" + DLL.ftLibVersionStr());
            Console.WriteLine("\nPlease, press the right or left button on ROBO TX to stop the demo...");

            //  library initialization
            uint errCode = DLL.InitLib();

            //  COM port name
            string ComPortName = args[0];
            Console.WriteLine(String.Format("\n\nOpen ComPort '{0}' ...", args[0]));

            //  open COM port
            ftHandle = DLL.OpenComDevice(ComPortName, 38400, ref errCode);

            if (errCode == (uint)FTERR.FTLIB_ERR_SUCCESS) {

                Console.WriteLine("Connected to ROBO TX Controller ...");

                //  init values in TransferArea to default
                InitTAValues();

                //  starting transferarea
                errCode = DLL.StartTransferArea(ftHandle);

                if (errCode == (uint)FTERR.FTLIB_ERR_SUCCESS) {

                    Console.WriteLine("TransferArea was started and runs...");

                    //  motor run and stop by distance < 15
                    MotorStop();
                    Thread.Sleep(50);

                    //  show message on ROBO TX display
                    string msg = "Demo program was terminated ...";
                    DLL.SetRoboTxMessage(ftHandle, (int)TA_ID.TA_LOCAL, msg);
                    Thread.Sleep(100);

                    //  stop TransferArea
                    DLL.StopTransferArea(ftHandle);
                }

                else {
                    //  error case
                    Console.WriteLine("Error: TransferArea was not started !");
                }

                //  close COM port
                Console.WriteLine("Closing ComPort '" + ComPortName + "' ...");
                errCode = DLL.CloseDevice(ftHandle);
            }

            else {
                //  error case
                Console.WriteLine("Error: No interface available (Port '" + ComPortName + "')");
            }

            //  close library
            DLL.CloseLib();
        }

        /*----------------------------------------------------------------------------- 
         *  CheckParameter  
         *---------------------------------------------------------------------------*/
        private static bool ChckCOMPar(string[] args) {

            if (args.Length > 0) {
                if (args[0].Length > 3) {
                    string sCOM = args[0].Substring(0,3);
                    if (sCOM.Equals("COM")) {
                        bool validNo;

                        string sNo = args[0].Substring(3);
                        try {
                            int iCOM = int.Parse(sNo);
                            validNo = (iCOM >= 1 && iCOM <= 255);
                        }
                        catch (Exception) {
                            Console.WriteLine("MotorStop.exe: invalid COM number...");
                            validNo = false;
                        }

                        if (!validNo)
                            Console.WriteLine("Usage: MotorStop.exe COMxx\t(e.g. COM2 or COM32)");
                        return validNo;
                    }
                }
            }

            Console.WriteLine("MotorStop.exe: no input given...");
            return false;
        }


        /*-----------------------------------------------------------------------------
         *  InitTAValues  
         *---------------------------------------------------------------------------*/
        private static void InitTAValues() {
            //  set all motors ON with duty= 0
            for (int mtrIdx = 0; mtrIdx < N_MOTOR; mtrIdx++) {
                DLL.SetMotorValues(ftHandle, (int)TA_ID.TA_LOCAL, mtrIdx, 0, 0, false);
                DLL.SetMotorConfig(ftHandle, (int)TA_ID.TA_LOCAL, mtrIdx, MOTOR_ON);
            }
        }

        /*-----------------------------------------------------------------------------
         *  MotorStop  
         *---------------------------------------------------------------------------*/
        private static void MotorStop() {

            bool MotorStopped, overrun=false;
            short distance=0;
            ushort count=0, cntmode=0;
            ushort leftBtn=0, rightBtn=0;
            int duty;


            //  set Motor 1 ON, run motor with duty= DUTY_MAX
            DLL.SetMotorValues(ftHandle, (int)TA_ID.TA_LOCAL, (int)MotorIdx.MOTOR_1, DUTY_MIN, 0, false);
            DLL.SetMotorConfig(ftHandle, (int)TA_ID.TA_LOCAL, (int)MotorIdx.MOTOR_1, MOTOR_ON);

            //  set Input I1 to 'Ultraschallsensor' mode
            DLL.SetUniConfig(ftHandle, (int)TA_ID.TA_LOCAL, (int)InpIdx.I1, (int)InputMode.MODE_ULTRASONIC, true);

            //  reset counter 1 (index=0)
            Console.Write("reset counter 1...");
            DLL.StartCounterReset(ftHandle, (int)TA_ID.TA_LOCAL, (int)CntIdx.COUNT_1);

            //  waiting for counter reset
            do
            {
                DLL.GetInCounterValue(ftHandle, (int)TA_ID.TA_LOCAL, (int)CntIdx.COUNT_1, ref count, ref cntmode);
                Thread.Sleep(1);
            } while (count > 0);
            Console.WriteLine("OK");
                        
            //  waiting for settings to ROBO TX Controller
            Thread.Sleep(100);

            //  init value
            MotorStopped = false;
            duty = DUTY_MAX;

            //  endless loop...
            //  if value of I1 (distance) is smaller than 15, the encoder motor stop,
            //  otherwise the motor runs with max speed
            while (true) {

                //  read input value and button delay from TransferArea
                DLL.GetUniInValue(ftHandle, (int)TA_ID.TA_LOCAL, (int)InpIdx.I1, ref distance, ref overrun);
                DLL.GetDisplayButton(ftHandle, (int)TA_ID.TA_LOCAL, ref leftBtn, ref rightBtn);

                //  check if the buttons were pressed
                if (leftBtn != 0 || rightBtn != 0) {
                    //  motor directly stop with brake 
                    DLL.SetMotorValues(ftHandle, (int)TA_ID.TA_LOCAL, (int)MotorIdx.MOTOR_1, 0, 0, true);
                    return;
                }

                //  set duty
                if ( distance < 15 ) {
                    MotorStopped = (duty == DUTY_MAX);
                    duty = DUTY_MIN;
                }
                else {
                    duty = DUTY_MAX;
                }

                //  set motor values
                DLL.SetMotorValues(ftHandle, (int)TA_ID.TA_LOCAL, (int)MotorIdx.MOTOR_1, duty, 0, false);

                if (MotorStopped) {

                    //  read counter value from TransferArea
                    DLL.GetInCounterValue(ftHandle, (int)TA_ID.TA_LOCAL, (int)CntIdx.COUNT_1, ref count, ref cntmode);

                    //  print the distance: value of counter 1
                    Console.WriteLine("Motor stopped, distance= " + distance + ", counter= " + count);

                    //  reset counter 1 (index=0)
                    DLL.StartCounterReset(ftHandle, (int)TA_ID.TA_LOCAL, (int)CntIdx.COUNT_1);
                    
                    //  reset flag
                    MotorStopped = false;
                }

                Thread.Sleep(5);
            }
        }

    }
}
